﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Diagnostics;
using System.Drawing;
using System.Drawing.Imaging;
using System.IO;
using System.Windows.Forms;
using System.Xml.Serialization;

namespace CubeDetection
{
   public static class DebugOut
   {
      private static DateTime dt = DateTime.Now;
      public static void Reset()
      {
         dt = DateTime.Now;
      }
      public static void Output(string Text)
      {
         Trace.WriteLine(Text + ": " + (DateTime.Now - dt));
      }
      public static void OutputReset(string Text)
      {
         Trace.WriteLine(Text + ": " + (DateTime.Now - dt));
         Reset();
      }
   }
   public class TimeStop
   {
      private DateTime dt = DateTime.Now;
      public TimeSpan Reset()
      {
         TimeSpan ts = DateTime.Now - dt;
         dt = DateTime.Now;
         return ts;
      }
      public TimeSpan TimeSpan()
      {
         TimeSpan ts = DateTime.Now - dt;
         return ts;
      }
   }
   public class EdgeCubeFilter
   {
      protected readonly List<CubeInfo> DetectedCubes;
      protected readonly int[,] FilterMask;
      protected readonly ImageFilter imageFilter;

      protected Bitmap bmp;
      protected CubeInfo[,] Cubes;
      protected int edgeIterations = 2;
      protected string filename;
      protected float maxDistance = 50f;
      protected int maxHeight = 65;
      protected int maxWidth = 65;
      protected int minHeight = 10;
      protected int minWidth = 10;
      protected int rasterX = 10;
      protected int rasterY = 10;
      protected Rectangle roi;
      protected Bitmap ScanBmp;
      protected Point[,] scanPoints;

      public EdgeCubeFilter()
      {
         FilterMask = new int[3,3];
         FilterMask[0, 0] = -1;
         FilterMask[0, 1] = 0;
         FilterMask[0, 2] = -1;
         FilterMask[1, 0] = 0;
         FilterMask[1, 1] = 4;
         FilterMask[1, 2] = 0;
         FilterMask[2, 0] = -1;
         FilterMask[2, 1] = 0;
         FilterMask[2, 2] = -1;
         imageFilter = new ImageFilter(FilterMask, 1);

         DetectedCubes = new List<CubeInfo>();
         EnableROI = false;
      }

      protected bool enableROI;
      public bool EnableROI
      {
         get { return enableROI; }
         set
         {
            enableROI = value;
            CalcScanPoints();
         }
      }

      public Bitmap Bitmap
      {
         get { return bmp; }
         set
         {
            if (value == null)
               return;
            bmp = value;
            if (roi.Width == 0 || roi.Right > value.Width)
               roi.Width = value.Width - roi.X;
            if (roi.Height == 0 || roi.Bottom > value.Height)
               roi.Height = value.Height - roi.Y;
            FindEdges();
            CalcScanPoints();
         }
      }

      public int MaxWidth
      {
         get { return maxWidth; }
         set { maxWidth = value; }
      }

      public float MaxDistance
      {
         get { return maxDistance; }
         set { maxDistance = value; }
      }

      public int MinWidth
      {
         get { return minWidth; }
         set { minWidth = value; }
      }


      //   this.nud_edge.DataBindings.Add(new System.Windows.Forms.Binding("Value", global::KukaCubeSolver.Properties.Settings.Default, "EdgeIteration", true, System.Windows.Forms.DataSourceUpdateMode.OnPropertyChanged));
      
      public int EdgeIterations
      {
         get { return edgeIterations; }
         set
         {
            if(edgeIterations == value)
               return;
            edgeIterations = value;
            FindEdges();
         }
      }

      public int MinHeight
      {
         get { return minHeight; }
         set { minHeight = value; }
      }

      public int MaxHeight
      {
         get { return maxHeight; }
         set { maxHeight = value; }
      }

      public int Threshold
      {
         get { return imageFilter.Threshold; }
         set
         {
            if (imageFilter.Threshold == value)
               return;
            imageFilter.Threshold = value;
            FindEdges();
         }
      }

      public int RasterX
      {
         get { return rasterX; }
         set
         {
            if(RasterX == value)
               return;
            rasterX = value;
            CalcScanPoints();
         }
      }

      public int RasterY
      {
         get { return rasterY; }
         set
         {
            if (rasterY == value)
               return;
            rasterY = value;
            CalcScanPoints();
         }
      }

      public Rectangle ROI
      {
         get { return roi; }
         set
         {
            if (roi.Equals(value))
               return;
            roi = value;
            CalcScanPoints();
         }
      }

      public Bitmap FilterPreview
      {
         get
         {
            var result = (Bitmap) ScanBmp.Clone();
            DrawRoi(ref result, Color.FromArgb(128, 255, 255, 255));
            return result;
         }
      }

      public Bitmap OriginalPreview
      {
         get
         {
            var result = (Bitmap) bmp.Clone();
            DrawRoi(ref result, Color.FromArgb(128, 0, 0, 0));
            return result;
         }
      }

      public unsafe Bitmap DetectedPreview
      {
         get
         {
            var result = (Bitmap) ScanBmp.Clone();
            //DrawRoi(ref result, Color.FromArgb(128, 255, 255, 255));

            //BitmapData bData = result.LockBits(new Rectangle(0, 0, result.Width, result.Height), ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);
            BitmapData bData = bmp.LockBits(new Rectangle(0, 0, bmp.Width, bmp.Height), ImageLockMode.ReadOnly,
                                            PixelFormat.Format24bppRgb);

            var pixelData = (byte*) bData.Scan0.ToPointer();
            rgb* rgbData;
            RGBStats rgbStats;
            int px1, px2, py1, py2;

            Graphics g = Graphics.FromImage(result);
            foreach (CubeInfo ci in DetectedCubes)
            {
               rgbStats = new RGBStats();

               px1 = px2 = ci.Cube.X + ci.Cube.Width/4;
               py1 = py2 = ci.Cube.Y + ci.Cube.Height/4;

               px2 += ci.Cube.Width/2;
               py2 += ci.Cube.Height/2;

               for (int y = py1; y <= py2; y++)
               {
                  rgbData = (rgb*) (&pixelData[y*bData.Stride]);
                  for (int x = px1; x <= px2; x++)
                  {
                     rgbStats.Add(rgbData[x]);
                  }
               }
               //g.DrawRectangle(new Pen(Color.FromArgb(127, 255, 255, 255)), ci.Cube);
               rgb CubeColor = rgbStats.Mean();
               ci.Color = Color.FromArgb(CubeColor.r, CubeColor.g, CubeColor.b);

               g.FillRectangle(new SolidBrush(ci.Color), new Rectangle(px1, py1, ci.Cube.Width/2, ci.Cube.Height/2));
               g.DrawRectangle(Pens.Black, new Rectangle(px1, py1, ci.Cube.Width/2, ci.Cube.Height/2));
               DrawCross(g, Pens.Red, ci.BaseX, ci.BaseY, result.Width, result.Height, 3);
            }
            bmp.UnlockBits(bData);
            return result;
         }
      }

      //private rgb borderColor = new rgb { b = 75, g = 75, r = 75 };
      //public rgb BorderColor
      //{
      //   get { return borderColor; }
      //   set { borderColor = value; }
      //}
      protected Color borderColor = Color.FromArgb(75, 75, 75);
      public Color BorderColor
      {
         get { return borderColor; }
         set { borderColor = value; }
      }

      protected void CalcScanPoints()
      {
         int xOff, yOff;
         float w, h;
         if (EnableROI)
         {
            w = roi.Width;
            h = roi.Height;
            xOff = roi.X;
            yOff = roi.Y;
         }
         else
         {
            if(bmp == null)
               return;
            w = bmp.Width;
            h = bmp.Height;
            xOff = 0;
            yOff = 0; 
         }
         float BaseX = w/(float) (rasterX + 1);
            float BaseY = h/(float) (rasterY + 1);

            scanPoints = new Point[rasterX,rasterY];
            for (int x = 0; x < rasterX; x++)
               for (int y = 0; y < rasterY; y++)
                  scanPoints[x, y] = new Point(xOff + (int) (BaseX*(1 + x)), yOff + (int) (BaseY*(1 + y)));

      }

      protected void FindEdges()
      {
         if (bmp == null)
            return;
         ScanBmp = (Bitmap) bmp.Clone();
         for (int i = 0; i < edgeIterations; i++)
            ScanBmp = imageFilter.Filter(ScanBmp);
      }

      protected void DrawRoi(ref Bitmap bitmap, Color color)
      {
         Graphics g = Graphics.FromImage(bitmap);

         for (int x = 0; x < rasterX; x++)
            for (int y = 0; y < rasterY; y++)
            {
               DrawCross(g, Pens.Red, scanPoints[x, y].X, scanPoints[x, y].Y, bitmap.Width, bitmap.Height, 3);
            }
         if (enableROI)
         {
            g.FillRectangle(new SolidBrush(color), 0, 0, bitmap.Width, roi.Y);
            g.FillRectangle(new SolidBrush(color), 0, roi.Bottom, bitmap.Width, bitmap.Height - roi.Bottom);
            g.FillRectangle(new SolidBrush(color), 0, roi.Y, roi.Left, roi.Height);
            g.FillRectangle(new SolidBrush(color), roi.Right, roi.Y, bitmap.Width - roi.Right, roi.Height);
         }
      }

      public static void DrawCross(Graphics g, Pen p, int x, int y, int w, int h, int size)
      {
         g.DrawLine(p, new Point(Math.Max(x - size, 0), y), new Point(Math.Min(x + size, w), y));
         g.DrawLine(p, new Point(x, Math.Max(y - size, 0)), new Point(x, Math.Min(y + size, w)));
      }

      public List<CubeInfo> ScanCubes()
      {

         Cubes = new CubeInfo[rasterX,rasterY];
         DetectedCubes.Clear();


        
         BitmapData bData = ScanBmp.LockBits(new Rectangle(0, 0, ScanBmp.Width, ScanBmp.Height), ImageLockMode.ReadOnly,
                                             PixelFormat.Format24bppRgb);
         for (int x = 0; x < RasterX; x++)
         {
            for (int y = 0; y < RasterY; y++)
            {
               Cubes[x, y] = new CubeInfo();
               FindPts(scanPoints[x, y].X, scanPoints[x, y].Y, ref Cubes[x, y], bData);
            }
         }

         ScanBmp.UnlockBits(bData);

         Graphics g = Graphics.FromImage(ScanBmp);
         for (int x = 0; x < RasterX; x++)
         {
            for (int y = 0; y < RasterY; y++)
            {
               if (!Cubes[x, y].Valid)
                  continue;
               if (Cubes[x, y].Cube.Width < minWidth || Cubes[x, y].Cube.Width > maxWidth ||
                   Cubes[x, y].Cube.Height < minHeight || Cubes[x, y].Cube.Height > maxHeight)
               {
                  Cubes[x, y].Valid = false;
                  continue;
               }
            }
         }


         for (int x = 0; x < RasterX; x++)
         {
            for (int y = 0; y < RasterY; y++)
            {
               bool intersect = false;
               for (int x1 = 0; x1 < RasterX; x1++)
                  for (int y1 = 0; y1 < RasterY; y1++)
                  {
                     if (intersect)
                        continue;
                     if (x1 == x && y1 == y)
                        continue;
                     if (!Cubes[x, y].Valid || !Cubes[x1, y1].Valid)
                        continue;
                     if (Cubes[x, y].Cube.IntersectsWith(Cubes[x1, y1].Cube))
                     {
                        if (Cubes[x, y].Cube.Width*Cubes[x, y].Cube.Height <
                            Cubes[x1, y1].Cube.Width*Cubes[x1, y1].Cube.Height)
                        {
                           intersect = true;
                           Cubes[x, y].Valid = false;
                        }
                        else if (Cubes[x, y].Cube.Width*Cubes[x, y].Cube.Height ==
                                 Cubes[x1, y1].Cube.Width*Cubes[x1, y1].Cube.Height)
                        {
                           Cubes[x1, y1].Valid = false;
                        }
                     }
                  }
               if (intersect)
                  continue;
               if (!Cubes[x, y].Valid)
                  continue;

               DetectedCubes.Add(Cubes[x, y]);
            }
         }

         DetectedCubes.Sort(new Comparison<CubeInfo>(CubeInfo.CompareCubeInfo));
         while (DetectedCubes.Count > 9)
         {
            DetectedCubes.Remove(DetectedCubes[DetectedCubes.Count - 1]);
         }
         DetectedCubes.Sort(new Comparison<CubeInfo>(CubeInfo.SortByPosition));


         return DetectedCubes;
      }

      protected bool IsBorder(rgb rgb)
      {
         return (rgb.r >= borderColor.R && rgb.g >= borderColor.G&& rgb.b >= borderColor.B);
         //return (rgb.r >= borderColor.r && rgb.g >= borderColor.g && rgb.b >= borderColor.b);

      }


      protected unsafe void FindPts(int x, int y, ref CubeInfo Cube, BitmapData bData)
      {
         Cube.BaseX = x;
         Cube.BaseY = y;
         var rgbData = (byte*) bData.Scan0.ToPointer();
         byte* rgbcData;

         var Stats = new RGBStats();

         rgb* pxData;
         rgb* pxcData = ((rgb*) &rgbData[y*bData.Stride]) + x;
         if (IsBorder(*pxcData))
         {
            Cube.Valid = false;
            return;
         }
         Stats.List.Add(*pxcData);
         var edgeP = new Point[4];
         double dist;

         RGBStats CurrentStats = null;

         //Oben
         for (int i = y - 1; i >= 0; i--)
         {
            pxData = (rgb*) &rgbData[i*bData.Stride];
            dist = HLineDist(pxData, x, 4, ScanBmp.Width, Stats.Mean(), out CurrentStats);
            //dist = distance(pxData[x], Stats.Mean() /** pxcData*/);
            if (dist > MaxDistance || IsBorder(pxData[x]))
            {
               edgeP[0] = new Point(x, i + 1);
               break;
            }
            else
            {
               Stats += CurrentStats;
            }
            if (dist < 15.0)
               Stats.Add(pxData[x]);
         }
         //Unten
         for (int i = y + 1; i < ScanBmp.Height; i++)
         {
            pxData = (rgb*) &rgbData[i*bData.Stride];
            dist = HLineDist(pxData, x, 4, ScanBmp.Width, Stats.Mean(), out CurrentStats);
            //dist = distance(pxData[x], Stats.Mean() /** pxcData*/);
            if (dist > MaxDistance || IsBorder(pxData[x]))
            {
               edgeP[1] = new Point(x, i - 1);
               break;
            }
            else
            {
               Stats += CurrentStats;
            }
            if (dist < 15.0)
               Stats.Add(pxData[x]);
         }

         //Links
         pxData = (rgb*) &rgbData[y*bData.Stride];
         for (int i = x - 1; i >= 0; i--)
         {
            dist = VLineDist(rgbData, i, y, 4, ScanBmp.Height, bData.Stride, Stats.Mean(), out CurrentStats);
            //dist = distance(pxData[i], Stats.Mean() /** pxcData*/);
            if (dist > MaxDistance || IsBorder(pxData[x]))
            {
               edgeP[2] = new Point(i + 1, y);
               break;
            }
            else
            {
               Stats += CurrentStats;
            }
            if (dist < 15.0)
               Stats.Add(pxData[x]);
         }

         //Rechts
         pxData = (rgb*) &rgbData[y*bData.Stride];
         for (int i = x + 1; i < ScanBmp.Width; i++)
         {
            dist = VLineDist(rgbData, i, y, 4, ScanBmp.Height, bData.Stride, Stats.Mean(), out CurrentStats);
            //dist = distance(pxData[i], Stats.Mean() /** pxcData*/);
            if (dist > MaxDistance || IsBorder(pxData[x]))
            {
               edgeP[3] = new Point(i - 1, y);
               break;
            }
            else
            {
               Stats += CurrentStats;
            }
            if (dist < 15.0)
               Stats.Add(pxData[x]);
         }

         Cube.Cube = new Rectangle(edgeP[2].X, edgeP[0].Y, edgeP[3].X - edgeP[2].X, edgeP[1].Y - edgeP[0].Y);
         Cube.Valid = true;
         rgb mean = Stats.Mean();
         Cube.Color = Color.FromArgb(mean.r, mean.g, mean.b);

      }

      protected unsafe double HLineDist(rgb* LineStartPtr, int x, int Delta, int ImgWidth, rgb Reference,
                                      out RGBStats Stats)
      {
         double DistMax = 0;
         double diff = 0;
         Stats = new RGBStats();
         for (int i = Math.Max(x - Delta, 0); i <= Math.Min(ImgWidth - 1, x + Delta); i++)
         {
            diff = distance(Reference, LineStartPtr[i]);
            DistMax = Math.Max(DistMax, diff);

            if (diff < 15.0)
               Stats.Add(LineStartPtr[i]);
         }
         return DistMax;
      }

      protected unsafe double VLineDist(byte* LineStartPtr, int x, int y, int Delta, int ImgHeight, int ImageStride,
                                      rgb Reference, out RGBStats Stats)
      {
         double DistMax = 0;
         double diff = 0;
         Stats = new RGBStats();
         for (int i = Math.Max(y - Delta, 0); i <= Math.Min(ImgHeight - 1, y + Delta); i++)
         {
            var cRGB = (rgb*) &LineStartPtr[i*ImageStride + x*sizeof (rgb)];
            diff = distance(Reference, *cRGB);
            DistMax = Math.Max(DistMax, diff);

            if (diff < 15.0)
               Stats.Add(*cRGB);
         }
         return DistMax;
      }

      protected double distance(rgb d1, rgb d2)
      {
         double d1d, d2d, d3d;
         d1d = d1.r - (double) d2.r;
         d2d = d1.g - (double) d2.g;
         d3d = d1.b - (double) d2.b;
         return Math.Sqrt(d1d*d1d + d2d*d2d + d3d*d3d);
      }

      protected double distance(Color d1, Color d2)
      {
         double d1d, d2d, d3d;
         d1d = d1.R - (double) d2.R;
         d2d = d1.G - (double) d2.G;
         d3d = d1.B - (double) d2.B;
         return Math.Sqrt(d1d*d1d + d2d*d2d + d3d*d3d);
      }

   }
}